package dhk;

import java.awt.Point;
import java.awt.geom.Point2D;

/**
 * Provides set of commonly used static methods related to robot movement.
 * 
 * @author Dean Kim
 * 
 */
public class TrackingUtility {

  /**
   * Computes TargetPosition for a given bearing and range.
   * 
   * @param source Source Point2D instance
   * @param bearing Bearing to target in degrees
   * @param range Range to target in pixel distance
   * @return Point2D instance containing the target position
   */
  protected static Point2D computeTargetPositFromBearingRange(Point2D source, double bearing,
      double range) {

    Point2D target = new Point2D.Double();

    // double xAxisDistance = Math.abs(Point2D.distance(source.getX(), 0, target.getX(), 0));

    double triangleAdjacentLength = Math.abs(Math.sin(Math.toRadians(bearing)) * range);
    double triangleOppositeLength = Math.abs(Math.cos(Math.toRadians(bearing)) * range);

    if ((bearing < 360) && (bearing >= 270)) {
      target.setLocation(source.getX() - triangleAdjacentLength, source.getY()
          + triangleOppositeLength);
    }

    if ((bearing < 270) && (bearing >= 180)) {
      target.setLocation(source.getX() - triangleAdjacentLength, source.getY()
          - triangleOppositeLength);
    }

    if ((bearing < 180) && (bearing >= 90)) {
      target.setLocation(source.getX() + triangleAdjacentLength, source.getY()
          - triangleOppositeLength);
    }

    if ((bearing < 90) && (bearing >= 0)) {
      target.setLocation(source.getX() + triangleAdjacentLength, source.getY()
          + triangleOppositeLength);
    }

    return target;
  }

  /**
   * Computes true bearing from a source to target.
   * 
   * @param source Source Point2D instance that provides source position
   * @param target Target Point2D instance that provides target position
   * @return True bearing to target position
   */
  protected static double computeFiringTrueBearing(Point2D source, Point2D target) {

    double triangleHypotenuseLength = Math.abs(Point.distance(source.getX(), source.getY(), target
        .getX(), target.getY()));
    double triangleAdjacentLength = Math.abs(Point.distance(source.getX(), 0, target.getX(), 0));
    double angle = Math.toDegrees(Math.acos(triangleAdjacentLength / triangleHypotenuseLength));
    double targetTrueBearing = 0;

    if ((target.getX() == source.getX()) && target.getY() > source.getY()) {
      targetTrueBearing = 0;
    }

    if ((target.getX() > source.getX()) && target.getY() == source.getY()) {
      targetTrueBearing = 90;
    }

    if ((target.getX() == source.getX()) && target.getY() < source.getY()) {
      targetTrueBearing = 180;
    }

    if ((target.getX() < source.getX()) && target.getY() == source.getY()) {
      targetTrueBearing = 270;
    }

    if ((target.getX() < source.getX()) && (target.getY() > source.getY())) {
      targetTrueBearing = angle + 270;
    }

    if ((target.getX() > source.getX()) && (target.getY() > source.getY())) {
      targetTrueBearing = 90 - angle;
    }

    if ((target.getX() > source.getX()) && (target.getY() < source.getY())) {
      targetTrueBearing = angle + 90;
    }

    if ((target.getX() < source.getX()) && (target.getY() < source.getY())) {
      targetTrueBearing = 270 - angle;
    }

    return targetTrueBearing;
  }

}
